//
// Created by fins on 24-10-25.
//

#ifndef LIDAR_TOOLKITS_WS_CUBE_H
#define LIDAR_TOOLKITS_WS_CUBE_H
#include <Eigen/Dense>
#include <geometry_msgs/msg/transform_stamped.hpp>

class Cube {
public:
    // 构造函数，通过中心和尺寸初始化立方体
    Cube();
    Cube(const Eigen::Vector3f& corner1, const Eigen::Vector3f& corner2);

    // 判断一个点是否在立方体内
    bool contains(const Eigen::Vector3f& point) const;

    // 通过 TransformStamped 移动和旋转立方体
    void transform(const geometry_msgs::msg::TransformStamped& transform);

    // 打印立方体的边界
    void print() const;

private:
    Eigen::Vector3f center; // 中心坐标
    Eigen::Vector3f size;   // 尺寸（宽、高、深）
    Eigen::Matrix3f rotation; // 旋转矩阵

    // 立方体六个面的方程参数
    float planes[6][4]; // 每个面存储 (A, B, C, D)

    float center2planes[6];

    Eigen::Vector3f center_planes[6];
};

#endif //LIDAR_TOOLKITS_WS_CUBE_H

